/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class DriveTestBot extends SimpleRobot {
    // The following variables give names to each PWM ID for the Jaguar
    // connected to the corresponding motor.
    final int MOTORID_FRONT_LEFT = 1;
    final int MOTORID_REAR_LEFT = 4;
    final int MOTORID_FRONT_RIGHT = 3;
    final int MOTORID_REAR_RIGHT = 2;

    // The following variable give a name to each Joystick channel. This makes
    // it easier to know in the driver station which joystick should be matched
    // to which joystick port.
    final int JOYSTICK_ATTACK_PORT = 1;

    //The following variables give names to joystick axes on the attack joystick
    private static final int JOYSTICK_ATTACK_AXIS_THUMB = 3;

    // The following variables give names to joystick buttons
    final int JOYSTICK_ATTACK_BUTTON_TRIGGER = 1;

    //The following variables give names to joystick axes on the attack joystick
    private static final int KINECT_AXIS_LEFT_ARM = 1;
    private static final int KINECT_AXIS_RIGHT_ARM = 2;

    // The following variables define names for the Jaguar motor controllers
    Jaguar jaguarFrontLeft = null;
    Jaguar jaguarFrontRight = null;
    Jaguar jaguarRearLeft = null;
    Jaguar jaguarRearRight = null;

    // The following variables define names for the Joysticks
    Joystick joystickAttack; //joystick used to enable Kinect control and set max drive speed
    KinectStick kinectRightArm; // kinect to be used like a joystick
    KinectStick kinectLeftArm; // kinect to be used like a joystick

    // Define a variable to point to the single DriverStation object
    DriverStation ds = DriverStation.getInstance();

    public DriveTestBot() {
        // create a joystick to be used by the driver
        joystickAttack = new Joystick(JOYSTICK_ATTACK_PORT);
         // create the kinect joystick
        kinectLeftArm = new KinectStick(KINECT_AXIS_LEFT_ARM);
        kinectRightArm = new KinectStick(KINECT_AXIS_RIGHT_ARM);
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        try {
            int motorID = 0;
            try {
                if(jaguarFrontLeft == null) {
                    motorID = MOTORID_FRONT_LEFT;
                    jaguarFrontLeft = new Jaguar(motorID);
                }
                if(jaguarFrontRight == null) {
                    motorID = MOTORID_FRONT_RIGHT;
                    jaguarFrontRight = new Jaguar(motorID);
                }
                if(jaguarRearLeft == null) {
                    motorID = MOTORID_REAR_LEFT;
                    jaguarRearLeft = new Jaguar(motorID);
                }
                if(jaguarRearRight == null) {
                    motorID = MOTORID_REAR_RIGHT;
                    jaguarRearRight = new Jaguar(motorID);
                }
            } catch (Exception ex) {
                System.out.println("Error initializing motorID " + motorID);
                ex.printStackTrace();
                System.out.println("Stack trace complete");
            }

            // create a robot drive system that consist of four motors
            //        drive = new RobotDrive(PWMChannelFrontLeftMotor, PWMChannelRearLeftMotor,
            //                PWMChannelFrontRightMotor, PWMChannelRearRightMotor);
            RobotDrive drive = new RobotDrive(jaguarFrontLeft, jaguarRearLeft, jaguarFrontRight, jaguarRearRight);

            // turn off motor safety
            drive.setSafetyEnabled(false);

            // declare variables for the stick inputs
            double rightValue;
            double leftValue;

            // Boost factor is a scale from 0 to 1 to indicate the maximum
            // power when not in boost
            double powerFactor = 0;

            // enable the watchdog
            // the watchdog will automatically disable the robot if something
            // in the software hangs. As long as you feed the watchdog, it
            // will assume everything is ok. However, if you go too long
            // without feeding the watchdog, it will assume the software is
            // hung and disable the robot
            // set Expiration of Watchdog as 1.0 second
            getWatchdog().setExpiration(1.0);
            getWatchdog().setEnabled(true);

            // loop over the following instructions as long as the robot
            // is enabled and the mode is set to teleoperated (operator control)
            while(isEnabled() && isOperatorControl()) {
                // always feed the watchdog first to let it know everything is ok
                getWatchdog().feed();

                // get the move and rotate values from the joystick
                rightValue = -kinectRightArm.getRawAxis(2);
                leftValue = -kinectLeftArm.getRawAxis(2);
                
                //if the trigger of the shooter stick is not pressed, disable RobotDrive
                if (joystickAttack.getRawButton(JOYSTICK_ATTACK_BUTTON_TRIGGER) == false) {
                    //Disable drivetain motors and stop the robot
                    powerFactor = 0;
                }
                else {
                    // get the max power by reading the thumb axis from the attack
                    // joystick. This will return a value between -1 and 1.
                    // We first invert the value because the joystick returns -1
                    // when the thumb is full forward and 1 when it is full aft.
                    // But we want the opposite polarity.
                    // We then change the scale to be 0 to 1.
                    powerFactor = -joystickAttack.getRawAxis(JOYSTICK_ATTACK_AXIS_THUMB);
                    powerFactor = (powerFactor + 1) / 2;
                }

                leftValue = leftValue * powerFactor;
                rightValue = rightValue * powerFactor;

                // Drive the robot in arcarde mode using the move and rotate values
                drive.tankDrive(leftValue, rightValue);

                //System.out.println("move: " + leftValue +
                        //" rotate = " + rightValue +
                        //" powerFactor = " + powerFactor +
                        //", frontLeft = " + jaguarFrontLeft.get() +
                        //", rearLeft = " + jaguarRearLeft.get() +
                        //", frontRight = " + jaguarFrontRight.get() +
                        //", rearRight = " + jaguarRearRight.get());

                // Sleep for 5 milliseconds to give the cRio a chance to
                // Process other events
                Timer.delay(.005);
            } // end of while loop
        } // end of try logic
        catch(Exception ex) {
            System.out.println("Unhandled exception in operatorControl, disabling");
            ex.printStackTrace();
        }
    }
}
